//
// Copyright 2019 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
// These materials are licensed under the Amazon Software License in connection with the Alexa Gadgets Program.
// The Agreement is available at https://aws.amazon.com/asl/.
// See the Agreement for the specific terms and conditions of the Agreement.
// Capitalized terms not defined in this file have the meanings given to them in the Agreement.
//

#include <stdio.h>
#include <assert.h>
#include <string.h>
#include "gadget_packet.h"

// helpers
static uint8_t packet_buffer[1024];
static size_t packet_buffer_len = 0;
static bool packet_error = FALSE;
static bool start_of_packet = FALSE;
static bool end_of_packet = FALSE;
static bool escape = FALSE;
static int packet_size = 0;

void cleanup() {
    packet_buffer_len = 0;
    packet_error = FALSE;
    start_of_packet = FALSE;
    end_of_packet = FALSE;
    escape = FALSE;
    packet_size = 0;
    memset(packet_buffer, 0, 1024);
}

static void verify_packet() {
    spp_directive_response_packet *resp = (spp_directive_response_packet*) packet_buffer;
    uint16_t index = 0;
    resp->header.sof = packet_buffer[index++];

    assert(resp->header.sof == SOP);

    resp->header.cmd = packet_buffer[index++];
    resp->header.err = packet_buffer[index++];
    resp->header.seqId = packet_buffer[index++];

    uint16_t index2 = 0;
    for (; index < packet_size - 3; index++)
    {
        resp->data[index2++] = packet_buffer[index];
    }

    spp_packet_directive_trailer* trailer = (spp_packet_directive_trailer*) (packet_buffer + index);
    assert(trailer->eof == EOP);
    //correct checksum (to big endian)
    trailer->checksum = (packet_buffer[index] << 8) | (packet_buffer[index+1]);
    printf("packet checksum = %d, trailer = %d\n", trailer->checksum, trailer->eof);
}

// This function is to demonstrate how to extract one packet from the receive buffer
static void read_packet_from_buffer(uint8_t * buffer, int buffer_len)
{
    // parse data from stream buffer
    int index = 0;
    for (index = 0; index < buffer_len; ++index) {
        // process every byte in the stream buffer
        if (packet_buffer_len >= 1024) {
            // packet too big or 0xf1 is dropped, we cannot process this packet
            if (packet_error == FALSE) {
                printf("Framing Error: Packet buffer overrun\n");
                packet_error = TRUE;
            }
        }

        if (packet_error == TRUE) {
            if (buffer[index] != 0xF0) {
                continue;
            }
            // if we found error, drop bytes until we see SOP
            cleanup();
        }

        switch(buffer[index]) {
        case SOP:
            if (start_of_packet == TRUE) {
                // SOP already received, this double SOP is not allowed
                printf("Framing Error: Received multiple SOP\n");
                packet_error = TRUE;
                continue;
            }
            if (packet_buffer_len != 0) {
                // SOP received out of order, it should be the start of packet.
                printf("Framing Error: SOP received in the middle of a packet\n");
                packet_error = TRUE;
                continue;
            }

            // no error
            start_of_packet = TRUE;
            packet_buffer[packet_buffer_len++] = SOP;
            ++packet_size;
            break;
        case EOP: // EOP
            end_of_packet = TRUE;
            packet_buffer[packet_buffer_len++] = EOP;
            ++packet_size;
            break;
        case ESP: // ESP
            escape = TRUE;
            break;
        default: // not special byte
            if (escape == TRUE) {
                // unescape
                buffer[index] ^= ESP;
                escape = FALSE;
            }
            packet_buffer[packet_buffer_len++] = buffer[index];
            ++packet_size;
        }

        if (end_of_packet == TRUE) {
            if (start_of_packet == FALSE) {
                printf("Framing Error: unexpected end of packet\n");
                packet_error = TRUE;
                continue;
            }
            else {
                // we got a full packet
                printf("process finished\n");
                verify_packet();
                // clean up for next packet processing
                cleanup();
            }
        }
    }
}

int main()
{
    // happy case, one packet per buffer
    uint8_t buf1[] = {0xf0, 0x02, 0x00, 0x01, 0x0a, 0x87, 0x02, 0x0a, 0x24, 0x0a, 0x0f, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x2e, 0x44, 0x69, 0x73, 0x63, 0x6f, 0x76, 0x65, 0x72, 0x79, 0x12, 0x11, 0x44, 0x69, 0x73, 0x63, 0x6f, 0x76, 0x65, 0x72, 0x2e, 0x52, 0x65, 0x73, 0x70, 0x6f, 0x6e, 0x73, 0x65, 0x12, 0xde, 0x01, 0x0a, 0xdb, 0x01, 0x0a, 0x0c, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x31, 0x32, 0x33, 0x34, 0x35, 0x12, 0x0b, 0x42, 0x45, 0x43, 0x75, 0x72, 0x69, 0x6f, 0x73, 0x69, 0x74, 0x79, 0x1a, 0x0c, 0x54, 0x69, 0x6e, 0x6b, 0x65, 0x72, 0x20, 0x54, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x06, 0x4b, 0x69, 0x64, 0x73, 0x49, 0x49, 0x5a, 0x53, 0x0a, 0x0e, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x49, 0x6e, 0x74, 0x65, 0x72, 0x66, 0x61, 0x63, 0x65, 0x12, 0x1a, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x2e, 0x47, 0x61, 0x64, 0x67, 0x65, 0x74, 0x2e, 0x53, 0x74, 0x61, 0x74, 0x65, 0x4c, 0x69, 0x73, 0x74, 0x65, 0x6e, 0x65, 0x72, 0x1a, 0x03, 0x31, 0x2e, 0x30, 0x22, 0x20, 0x0a, 0x0a, 0x0a, 0x08, 0x77, 0x61, 0x6b, 0x65, 0x77, 0x6f, 0x72, 0x64, 0x0a, 0x08, 0x0a, 0x06, 0x74, 0x69, 0x6d, 0x65, 0x72, 0x73, 0x0a, 0x08, 0x0a, 0x06, 0x61, 0x6c, 0x61, 0x72, 0x6d, 0x73, 0x62, 0x53, 0x0a, 0x01, 0x31, 0x12, 0x20, 0x6e, 0x6a, 0x21, 0x26, 0x8e, 0xa9, 0xea, 0xfb, 0xf2, 0x03, 0x56, 0x5e, 0x58, 0x80, 0xeb, 0xc6, 0xfe, 0x03, 0xa3, 0xb9, 0xda, 0x6e, 0xc9, 0xef, 0x94, 0x15, 0x47, 0x2f, 0x90, 0xcf, 0xae, 0xcd, 0xad, 0x1a, 0x01, 0x31, 0x22, 0x0e, 0x41, 0x31, 0x56, 0x35, 0x33, 0x31, 0x48, 0x46, 0x59, 0x30, 0x5a, 0x4f, 0x42, 0x35, 0x2a, 0x0b, 0x42, 0x45, 0x43, 0x75, 0x72, 0x69, 0x6f, 0x73, 0x69, 0x74, 0x79, 0x32, 0x0c, 0x32, 0x30, 0x37, 0x33, 0x35, 0x62, 0x30, 0x37, 0x36, 0x39, 0x33, 0x31, 0x56, 0xbf, 0xf1};

    // sticky packets, two packets per buffer
    uint8_t buf2[] = {0xf0, 0x02, 0x00, 0x01, 0x0a, 0x87, 0x02, 0x0a, 0x24, 0x0a, 0x0f, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x2e, 0x44, 0x69, 0x73, 0x63, 0x6f, 0x76, 0x65, 0x72, 0x79, 0x12, 0x11, 0x44, 0x69, 0x73, 0x63, 0x6f, 0x76, 0x65, 0x72, 0x2e, 0x52, 0x65, 0x73, 0x70, 0x6f, 0x6e, 0x73, 0x65, 0x12, 0xde, 0x01, 0x0a, 0xdb, 0x01, 0x0a, 0x0c, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x31, 0x32, 0x33, 0x34, 0x35, 0x12, 0x0b, 0x42, 0x45, 0x43, 0x75, 0x72, 0x69, 0x6f, 0x73, 0x69, 0x74, 0x79, 0x1a, 0x0c, 0x54, 0x69, 0x6e, 0x6b, 0x65, 0x72, 0x20, 0x54, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x06, 0x4b, 0x69, 0x64, 0x73, 0x49, 0x49, 0x5a, 0x53, 0x0a, 0x0e, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x49, 0x6e, 0x74, 0x65, 0x72, 0x66, 0x61, 0x63, 0x65, 0x12, 0x1a, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x2e, 0x47, 0x61, 0x64, 0x67, 0x65, 0x74, 0x2e, 0x53, 0x74, 0x61, 0x74, 0x65, 0x4c, 0x69, 0x73, 0x74, 0x65, 0x6e, 0x65, 0x72, 0x1a, 0x03, 0x31, 0x2e, 0x30, 0x22, 0x20, 0x0a, 0x0a, 0x0a, 0x08, 0x77, 0x61, 0x6b, 0x65, 0x77, 0x6f, 0x72, 0x64, 0x0a, 0x08, 0x0a, 0x06, 0x74, 0x69, 0x6d, 0x65, 0x72, 0x73, 0x0a, 0x08, 0x0a, 0x06, 0x61, 0x6c, 0x61, 0x72, 0x6d, 0x73, 0x62, 0x53, 0x0a, 0x01, 0x31, 0x12, 0x20, 0x6e, 0x6a, 0x21, 0x26, 0x8e, 0xa9, 0xea, 0xfb, 0xf2, 0x03, 0x56, 0x5e, 0x58, 0x80, 0xeb, 0xc6, 0xfe, 0x03, 0xa3, 0xb9, 0xda, 0x6e, 0xc9, 0xef, 0x94, 0x15, 0x47, 0x2f, 0x90, 0xcf, 0xae, 0xcd, 0xad, 0x1a, 0x01, 0x31, 0x22, 0x0e, 0x41, 0x31, 0x56, 0x35, 0x33, 0x31, 0x48, 0x46, 0x59, 0x30, 0x5a, 0x4f, 0x42, 0x35, 0x2a, 0x0b, 0x42, 0x45, 0x43, 0x75, 0x72, 0x69, 0x6f, 0x73, 0x69, 0x74, 0x79, 0x32, 0x0c, 0x32, 0x30, 0x37, 0x33, 0x35, 0x62, 0x30, 0x37, 0x36, 0x39, 0x33, 0x31, 0x56, 0xbf, 0xf1, 0xf0, 0x02, 0x00, 0x01, 0x0a, 0x87, 0x02, 0x0a, 0x24, 0x0a, 0x0f, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x2e, 0x44, 0x69, 0x73, 0x63, 0x6f, 0x76, 0x65, 0x72, 0x79, 0x12, 0x11, 0x44, 0x69, 0x73, 0x63, 0x6f, 0x76, 0x65, 0x72, 0x2e, 0x52, 0x65, 0x73, 0x70, 0x6f, 0x6e, 0x73, 0x65, 0x12, 0xde, 0x01, 0x0a, 0xdb, 0x01, 0x0a, 0x0c, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x31, 0x32, 0x33, 0x34, 0x35, 0x12, 0x0b, 0x42, 0x45, 0x43, 0x75, 0x72, 0x69, 0x6f, 0x73, 0x69, 0x74, 0x79, 0x1a, 0x0c, 0x54, 0x69, 0x6e, 0x6b, 0x65, 0x72, 0x20, 0x54, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x06, 0x4b, 0x69, 0x64, 0x73, 0x49, 0x49, 0x5a, 0x53, 0x0a, 0x0e, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x49, 0x6e, 0x74, 0x65, 0x72, 0x66, 0x61, 0x63, 0x65, 0x12, 0x1a, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x2e, 0x47, 0x61, 0x64, 0x67, 0x65, 0x74, 0x2e, 0x53, 0x74, 0x61, 0x74, 0x65, 0x4c, 0x69, 0x73, 0x74, 0x65, 0x6e, 0x65, 0x72, 0x1a, 0x03, 0x31, 0x2e, 0x30, 0x22, 0x20, 0x0a, 0x0a, 0x0a, 0x08, 0x77, 0x61, 0x6b, 0x65, 0x77, 0x6f, 0x72, 0x64, 0x0a, 0x08, 0x0a, 0x06, 0x74, 0x69, 0x6d, 0x65, 0x72, 0x73, 0x0a, 0x08, 0x0a, 0x06, 0x61, 0x6c, 0x61, 0x72, 0x6d, 0x73, 0x62, 0x53, 0x0a, 0x01, 0x31, 0x12, 0x20, 0x6e, 0x6a, 0x21, 0x26, 0x8e, 0xa9, 0xea, 0xfb, 0xf2, 0x03, 0x56, 0x5e, 0x58, 0x80, 0xeb, 0xc6, 0xfe, 0x03, 0xa3, 0xb9, 0xda, 0x6e, 0xc9, 0xef, 0x94, 0x15, 0x47, 0x2f, 0x90, 0xcf, 0xae, 0xcd, 0xad, 0x1a, 0x01, 0x31, 0x22, 0x0e, 0x41, 0x31, 0x56, 0x35, 0x33, 0x31, 0x48, 0x46, 0x59, 0x30, 0x5a, 0x4f, 0x42, 0x35, 0x2a, 0x0b, 0x42, 0x45, 0x43, 0x75, 0x72, 0x69, 0x6f, 0x73, 0x69, 0x74, 0x79, 0x32, 0x0c, 0x32, 0x30, 0x37, 0x33, 0x35, 0x62, 0x30, 0x37, 0x36, 0x39, 0x33, 0x31, 0x56, 0xbf, 0xf1};

    // funky pakcets, one packet split into two buffers
    uint8_t buf3[] = {0xf0, 0x02, 0x00, 0x01, 0x0a, 0x87, 0x02, 0x0a, 0x24, 0x0a, 0x0f, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x2e, 0x44, 0x69, 0x73, 0x63, 0x6f, 0x76, 0x65, 0x72, 0x79, 0x12, 0x11, 0x44, 0x69, 0x73, 0x63, 0x6f, 0x76, 0x65, 0x72, 0x2e, 0x52, 0x65, 0x73, 0x70, 0x6f, 0x6e, 0x73, 0x65, 0x12, 0xde, 0x01, 0x0a, 0xdb, 0x01, 0x0a, 0x0c, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x31, 0x32, 0x33, 0x34, 0x35, 0x12, 0x0b, 0x42, 0x45, 0x43, 0x75, 0x72, 0x69, 0x6f, 0x73, 0x69, 0x74, 0x79, 0x1a, 0x0c, 0x54, 0x69, 0x6e, 0x6b, 0x65, 0x72, 0x20, 0x54, 0x61, 0x62, 0x6c, 0x65, 0x22, 0x06, 0x4b, 0x69, 0x64, 0x73, 0x49, 0x49, 0x5a, 0x53, 0x0a, 0x0e, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x49};
    uint8_t buf4[] = {0x6e, 0x74, 0x65, 0x72, 0x66, 0x61, 0x63, 0x65, 0x12, 0x1a, 0x41, 0x6c, 0x65, 0x78, 0x61, 0x2e, 0x47, 0x61, 0x64, 0x67, 0x65, 0x74, 0x2e, 0x53, 0x74, 0x61, 0x74, 0x65, 0x4c, 0x69, 0x73, 0x74, 0x65, 0x6e, 0x65, 0x72, 0x1a, 0x03, 0x31, 0x2e, 0x30, 0x22, 0x20, 0x0a, 0x0a, 0x0a, 0x08, 0x77, 0x61, 0x6b, 0x65, 0x77, 0x6f, 0x72, 0x64, 0x0a, 0x08, 0x0a, 0x06, 0x74, 0x69, 0x6d, 0x65, 0x72, 0x73, 0x0a, 0x08, 0x0a, 0x06, 0x61, 0x6c, 0x61, 0x72, 0x6d, 0x73, 0x62, 0x53, 0x0a, 0x01, 0x31, 0x12, 0x20, 0x6e, 0x6a, 0x21, 0x26, 0x8e, 0xa9, 0xea, 0xfb, 0xf2, 0x03, 0x56, 0x5e, 0x58, 0x80, 0xeb, 0xc6, 0xfe, 0x03, 0xa3, 0xb9, 0xda, 0x6e, 0xc9, 0xef, 0x94, 0x15, 0x47, 0x2f, 0x90, 0xcf, 0xae, 0xcd, 0xad, 0x1a, 0x01, 0x31, 0x22, 0x0e, 0x41, 0x31, 0x56, 0x35, 0x33, 0x31, 0x48, 0x46, 0x59, 0x30, 0x5a, 0x4f, 0x42, 0x35, 0x2a, 0x0b, 0x42, 0x45, 0x43, 0x75, 0x72, 0x69, 0x6f, 0x73, 0x69, 0x74, 0x79, 0x32, 0x0c, 0x32, 0x30, 0x37, 0x33, 0x35, 0x62, 0x30, 0x37, 0x36, 0x39, 0x33, 0x31, 0x56, 0xbf, 0xf1};

    read_packet_from_buffer(buf1, sizeof(buf1)/sizeof(buf1[0]));
    read_packet_from_buffer(buf2, sizeof(buf2)/sizeof(buf2[0]));
    read_packet_from_buffer(buf3, sizeof(buf3)/sizeof(buf3[0]));
    read_packet_from_buffer(buf4, sizeof(buf4)/sizeof(buf4[0]));
}
